/*
 *  polygonizer.h
 *  Fracture
 *
 *  Created by Jamie Portsmouth on 08/05/2011.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#if 0


#ifndef POLYGONIZER_H
#define POLYGONIZER_H

#include <list>
#include <vector>
#include "Vec4.h"
#include "Matrix3.h"
#include "Aabb.h"
#include "float.h"
#include "Geometry.h"
#include <cmath>

#include "AabbTree.h"


// FOR DEBUGGING PURPOSES
#include <OpenGL/OpenGL.h>
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>


using namespace std;
using namespace Imath;


float ran2(long *idum);

struct ExampleLevelSetFunctor
{
	// Level set at value=0.0
	static float value(Vec4 P)
	{
		float X = P[0];
		float Y = P[1];
		float Z = P[2];
		return X*X + Y*Y + Z*Z;
	}
};


class Polygonizer 
{
public:
		
	// Pass the functor which defines the level set as the zero value isosurface
	template<typename Func>
	void polygonize(const Aabb& bbox, const float tolerance);

	const Geometry& getGeometry()
	{
		return m_mesh;
	}
	
	struct BoundaryTriangle
	{
		BoundaryTriangle(int a, int b, int c=-1, int ownerTriangle=-1) : m_a(a), m_b(b), m_otherTriangleVertex(c), m_ownerTriangle(ownerTriangle) {}
		int m_a, m_b;
		int m_otherTriangleVertex;
		int m_ownerTriangle;
	};
	
	
	struct Edge
	{
		Edge(Vec4 a, Vec4 b) 
		{
			m_a = V3f(a[0], a[1], a[2]);
			m_b = V3f(b[0], b[1], b[2]);
		}
		Edge(V3f a, V3f b) : m_a(a), m_b(b) {}
		Imath::V3f m_a, m_b;
	};
	
	static inline float edgeEdgeDistance(const Edge& edge1, const Edge& edge2,
										 float &s, float&t, V3f& c1, V3f& c2)
	{
		const Imath::V3f& p1 = edge1.m_a;
		const Imath::V3f& q1 = edge1.m_b;
		const Imath::V3f& p2 = edge2.m_a;
		const Imath::V3f& q2 = edge2.m_b;
		
		V3f d1 = q1 - p1; // Direction vector of segment S1
		V3f d2 = q2 - p2; // Direction vector of segment S2
		
		V3f r = p1 - p2;
		float a = d1 ^ d1; // Squared length of segment S1, always nonnegative
		float e = d2 ^ d2; // Squared length of segment S2, always nonnegative
		float f = d2 ^ r;
		
		// Check if either or both segments degenerate into points
		if (a<=FLT_EPSILON && e<=FLT_EPSILON)
		{
			// Both segments degenerate into points
			s = t = 0.0f;
			c1 = p1;
			c2 = p2;
			return (c1-c2)^(c1-c2);
		}
		
		if (a<=FLT_EPSILON)
		{
			// First segment degenerates into a point
			s = 0.0f;
			t = f/e;
			t = clamp(t, 0.0f, 1.0f);
		}
		else
		{
			float c = d1^r;
			if (e<=FLT_EPSILON)
			{
				// Second segment degenerates into a point
				t = 0.0f;
				s = clamp(-c/a, 0.0f, 1.0f);
			}
			else
			{
				// The general nondegenerate case starts here
				float b = d1^d2;
				float denom = a*e - b*b;
				
				// If segments non parallel, compute closest point on L1 to L2 and
				// clamp to segment S1. Else pick arbitrary s (here 0)
				if (denom != 0.0f)
					s = clamp((b*f - c*e)/denom, 0.0f, 1.0f);
				else 
					s = 0.0f;
				
				// Compute point on L2 closest to S1(s)
				t = (b*s + f)/e;
				
				// If t in [0,1] done. Else clamp t, recompute s for the new value of t
				// and clamp s to [0,1]
				if (t<0.0f)
				{
					t = 0.0f;
					s = clamp(-c/a, 0.0f, 1.0f);
				}
				else if (t>1.0f) 
				{
					t = 1.0f;
					s = clamp((b-c)/a, 0.0f, 1.0f);
				}
			}
		}
		
		c1 = p1 + d1*s;
		c2 = p2 + d2*t;
		return (c1-c2)^(c1-c2);
	}
	
	
	struct Front
	{
		// Linked list of edges in the growing front
		list<BoundaryTriangle> m_boundaryTriangles;
		
		/*
		void rebuildBVH()
		{
			vector<Box3f> leafAabbs;
			vector<int> leafDatas;
			
			list<Edge>::iterator edgeIter = m_edges.begin();
			while (edgeIter!=m_edges.end())
			{
				const Edge& edge = *edgeIter;
				const Vec4& meshVertexA = m_mesh.m_vertices[edge.m_a];
				const Vec4& meshVertexB = m_mesh.m_vertices[edge.m_b];
				
				V3f vA(meshVertexA[0], meshVertexA[1], meshVertexA[2]);
				V3f vB(meshVertexB[0], meshVertexB[1], meshVertexB[2]);
				Box3f box;
				box.extendBy(vA);
				box.extendBy(vB);
				
				leafAabbs.push_back(box);
				leafDatas.push_back(edgeIter);				
				edgeIter++;
			}
			
			m_BVH.clear();
			m_BVH.buildTreeTopDown(leafAabbs, leafDatas);
		}
		
		bool collideEdge(const Edge& testEdge, float tolerance)
		{
			const Vec4& testEdgeA = m_mesh.m_vertices[testEdge.m_a];
			const Vec4& testEdgeB = m_mesh.m_vertices[testEdge.m_b];
			
			V3f vA(testEdgeA[0], testEdgeA[1], testEdgeA[2]);
			V3f vB(testEdgeB[0], testEdgeB[1], testEdgeB[2]);
			Box3f testAabb;
			testAabb.extendBy(testEdgeA);
			testAabb.extendBy(testEdgeB);
			
			vector<int> leafNodeIndices;
			m_BVH.query(testAabb, leafNodeIndices);
			
			for (int i=0; i<leafNodeIndices.size(); ++i)
			{
				list<Edge>::iterator edgeIter = m_BVH.getLeafData(leafNodeIndices[i]);
				const Edge& hitEdge = *edgeIter;
				if (edgeEdgeHit(testEdge, hitEdge, m_mesh, tolerance)) return true;
			}
			
			return false;
		}
		
		AabbTree<list<Edge>::iterator> m_BVH;
		 */
	};
		
	vector<Front> m_fronts;
	
	
	/*
		New algorithm:
	 
		- When a new triangle is added to the front, there is either 2 edges added or 1 (the latter case if the new triangle is formed
	    just by connecting existing vertices).
		- The new edge(s) are subdivided into M 'egde segments'.
		- These segments are projected onto the surface (by projecting their endpoints).
	    - We generate a new front Edge, with:
	
				- the indices of the edge mesh vertex endpoints
				- the edge segment endpoints
				- the bounding box of the whole set of edge segments for this edge
				- A BVH of the edge segments
	
	 - We add the bounding box of the projected edge to the edge BVH
	 
		- With this data structure
		- We decide whether a new triangle can be added as follows:
			
			- decide on a distance tolerance for hits. A decent choise may be something like, the dot product: 
						
					epsilon = grad(f) ^ (segment vec.).
	 
			- compute the edge segments for the new triangle, and their bounding boxes and BVH
			- if there are any (edge-edge) epsilon-close hits, collide the segments BVHs of both edges in the hit.
			- if there are any (segment-segment) epsilon-close hits, collide the segments
			- do the narrowphase segment-segment collision, using the epsilon tolerance.
			- if there are no hits, we can go ahead and add the triangle.
	 
	 */
	
	
	
	

	class EdgeBVH
	{
	public:
		
		// Data structure to accelerate collision detection with a mesh which is growing dynamically
		// (i.e. edges are being appended to it, but not removed).
		// As edges are inserted, every numEdgesPerSubTree inserted it places the newly inserted edges into a two level BVH.
		// The most recently inserted edges are kept in a small ("dynamic") BVH which is rebuilt on each insertion.
				
		EdgeBVH(int numEdgesPerSubTree) : m_numEdgesPerSubTree(numEdgesPerSubTree) {}
		
		~EdgeBVH()
		{
			for (int n=0; n<m_subTrees.size(); ++n)
			{
				delete m_subTrees[n];
			}
		}
		
		// Given a test AABB, return the edges which it (potentially) overlaps
		void query( const Box3f& testAabb, vector<Edge>& edges ) const
		{
			vector<int> leafIndices;
			vector<int> subTreeIndices;
			m_globalTree.query(testAabb, subTreeIndices);
			for (int m=0; m<subTreeIndices.size(); ++m)
			{
				int subTreeIndex = subTreeIndices[m];
				assert(subTreeIndex>=0 && subTreeIndex<m_subTrees.size());
				const AabbTree<Edge>* subTree = m_subTrees[subTreeIndex];
				subTree->query(testAabb, leafIndices);
				for (int l=0; l<leafIndices.size(); ++l)
				{
					edges.push_back(subTree->getLeafData(leafIndices[l]));
				}
			}
			{
				m_dynamicTree.query(testAabb, leafIndices);
				for (int l=0; l<leafIndices.size(); ++l)
				{
					edges.push_back(m_dynamicTree.getLeafData(leafIndices[l]));
				}
			}
		}
				
		// Insert an edge, which subsequently will be available for overlap queries
		void insertEdge(const Edge& edge)
		{
			m_latestEdges.push_back(edge);
			
			if (m_latestEdges.size() == m_numEdgesPerSubTree)
			{
				// Build a new subtree
				AabbTree<Edge>* subtree = new AabbTree<Edge>();
				_build(subtree);
				m_subTrees.push_back(subtree);
				m_latestEdges.clear();
			
				// Rebuild the global tree to include the new subtree
				{
					vector<Box3f> leafAabbs;
					vector<int> leafDatas;
					int M = m_subTrees.size();
					for (int m=0; m<M; ++m)
					{
						const AabbTree<Edge>* subtree = m_subTrees[m];
						leafAabbs.push_back(subtree->getRootAabb());
						leafDatas.push_back(m);
					}
					m_globalTree.buildTreeTopDown(leafAabbs, leafDatas);
				}
				
				// Empty the dynamic tree
				m_dynamicTree.clear();
			}
			else
			{
				// Rebuild the dynamic tree
				_build(&m_dynamicTree);
			}
		}
		
		private:
		
			int m_numEdgesPerSubTree;
			
			AabbTree<int> m_globalTree; 
			AabbTree<Edge> m_dynamicTree;
			vector<AabbTree<Edge>*> m_subTrees;
			
			vector<Edge> m_latestEdges;
		
			void _build(AabbTree<Edge>* tree)
			{
				vector<Box3f> leafAabbs;
				vector<Edge> leafDatas;
				for (int n=0; n<m_latestEdges.size(); ++n)
				{
					const Edge& edge = m_latestEdges[n];
					V3f vA(edge.m_a[0], edge.m_a[1], edge.m_a[2]);
					V3f vB(edge.m_b[0], edge.m_b[1], edge.m_b[2]);
					Box3f box;
					box.extendBy(vA);
					box.extendBy(vB);
					leafAabbs.push_back(box);
					leafDatas.push_back(edge);
				}
				tree->buildTreeTopDown(leafAabbs, leafDatas);
			}
	};
	
	EdgeBVH m_BVH;
	
	long m_seed;
	
	template<typename Func>
	inline Vec4 gradient(const Vec4& point, float e, float inv2E)
	{
		Vec4 dX = Vec4(e,0,0);
		Vec4 dY = Vec4(0,e,0);
		Vec4 dZ = Vec4(0,0,e);
		float Vxp = Func::value(point+dX);
		float Vxm = Func::value(point-dX);
		float Vyp = Func::value(point+dY);
		float Vym = Func::value(point-dY);
		float Vzp = Func::value(point+dZ);
		float Vzm = Func::value(point-dZ);
		float gradX = inv2E * (Vxp - Vxm);
		float gradY = inv2E * (Vyp - Vym);
		float gradZ = inv2E * (Vzp - Vzm);
		return Vec4(gradX, gradY, gradZ);
	}
	
	template<typename Func>
	inline Matrix3 hessian(const Vec4& point, float e, float inv2E)
	{
		Vec4 dX = Vec4(e,0,0);
		Vec4 dY = Vec4(0,e,0);
		Vec4 dZ = Vec4(0,0,e);
		Vec4 Gxp = gradient<Func>(point + dX, e, inv2E);
		Vec4 Gxm = gradient<Func>(point - dX, e, inv2E);
		Vec4 Gyp = gradient<Func>(point + dY, e, inv2E);
		Vec4 Gym = gradient<Func>(point - dY, e, inv2E);
		Vec4 Gzp = gradient<Func>(point + dZ, e, inv2E);
		Vec4 Gzm = gradient<Func>(point - dZ, e, inv2E);
		
		float hr = 0.5f*inv2E;
		float hXX = inv2E * (Gxp[0] - Gxm[0]);
		float hYY = inv2E * (Gyp[1] - Gym[1]); 
		float hZZ = inv2E * (Gzp[2] - Gzm[2]);
		float hXY = hr * (Gxp[1] - Gxm[1] + Gyp[0] - Gym[0]);
		float hYZ = hr * (Gyp[2] - Gym[2] + Gzp[1] - Gzm[1]);
		float hXZ = hr * (Gzp[0] - Gzm[0] + Gxp[2] - Gxm[2]);
		
		Vec4 col0(hXX, hXY, hXZ);
		Vec4 col1(hXY, hYY, hXY);
		Vec4 col2(hXZ, hYZ, hZZ);
		Matrix3 H;
		H.setColumn(0, col0);
		H.setColumn(1, col1);
		H.setColumn(2, col2);
		return H;
	}
	
	
	template<typename Func>
	Vec4 project(const Vec4& start, float tol, float distanceEpsilon, int maxIterations)
	{
		float inv2D = 0.5f/distanceEpsilon;
		float tolSqr = tol*tol;
		
		Vec4 x = start;
		float f = Func::value(x);
		float fSqr = f*f;

		int nIter = 0;
		
		// Take Newton steps until the function magnitude falls below tolerance
		while (fSqr>tolSqr && nIter<maxIterations)
		{
			// (for the moment, ignore the issue of what to do if the gradient is close to zero).
			Vec4 grad = gradient<Func>(x, distanceEpsilon, inv2D);
			//if ( abs(grad[0]) < tol ) { printf("project() failed, gradient component too small"); exit(1); }
			//if ( abs(grad[1]) < tol ) { printf("project() failed, gradient component too small"); exit(1); }
			//if ( abs(grad[2]) < tol ) { printf("project() failed, gradient component too small"); exit(1); }
			
			// Take a Newton step 
			Vec4 step = f*grad/(grad*grad);
			
			// Reduce step size until the function magnitude falls, if needed 
			float fp, fpSqr;
			Vec4 xp;
			do 
			{
				nIter++;
				xp = x - step;
				fp = Func::value(xp);
				fpSqr = fp*fp;
				step *= 0.5f;
			}
			while (fpSqr>=fSqr && nIter<maxIterations);
			
			x = xp;
			f = fp;
			fSqr = fpSqr;
		}
		
		if (nIter>=maxIterations) 
		{ 
			printf("project() failed, too many iterations"); 
			exit(1); 
		}

		return x;
	}

	
	static inline bool intersectRaySphere(const Vec4& p, const Vec4& d, float R, const Vec4& C)
	{
		Vec4 m = p - C;
		float b = m*d;
		float c = m*m - R*R;
		if (c>0.0f && b>0.0f) return false;
		float discr = b*b - (d*d)*c;
		if (discr<0.0f) return false;
		float tlo = -b - sqrt(discr); 
		float thi = -b + sqrt(discr);
		if (tlo>=0.0f && thi<=d*d) return true;
		return false;
	}
	
	
	static inline void computeCircumcircle(const Vec4& A, const Vec4& B, const Vec4& C,
										   float& circumradius, Vec4& circumcenter)
	{
		// Using formulae from http://en.wikipedia.org/wiki/Circumscribed_circle
		Vec4 a = A - C;
		Vec4 b = B - C;
		float al = a.length3();
		float bl = b.length3();
		Vec4 acrossb = a % b;
		circumradius = al * bl * (a-b).length3() / (2.f * acrossb.length3());
		circumcenter = C + (((al*al)*b - (bl*bl)*a) % acrossb) / (2.f * (acrossb*acrossb));
	}
	
	template<typename Func>
	bool grow()
	{
		return grow<Func>(m_fronts[0], m_mesh, m_distanceEpsilon, m_tol);
	}
	
	bool edgeFits(const Edge& Ein)
	{
		Edge E = Ein;
		
		// TO DO: come up with a proper tolerance:
		// Most likely have to subdivide edge into say 16 segments to get reasonable results.
		// 
		float l = (E.m_b - E.m_a).length() + FLT_EPSILON;
		float tolerance = 0.05f * l; 
		
		// Snip off the ends of the edge, to avoid spurious hits with its neighbour edges
		V3f n = (E.m_b - E.m_a)/l;
		
		//const float R = 0.01f*tolerance;
		//const float Rsqr = R*R;
		
		E.m_a += 2.f * n * tolerance;
		E.m_b -= 2.f * n * tolerance;
		
		Box3f testBox;
		testBox.extendBy(E.m_a);
		testBox.extendBy(E.m_b);

		vector<Edge> hitEdges;
		m_BVH.query(testBox, hitEdges);
				
		for (int e=0; e<hitEdges.size(); ++e)
		{
			const Edge& hitEdge = hitEdges[e];
			float s, t;
			V3f c1, c2;
			if ( edgeEdgeDistance(E, hitEdge, s, t, c1, c2) < tolerance*tolerance )
			{
				//bool a1 = ((c1-E.m_a)^(c1-E.m_a)) < Rsqr;
				//bool a2 = ((c2-E.m_a)^(c2-E.m_a)) < Rsqr;
				
				//bool b2 = ((c2-E.m_b)^(c2-E.m_b)) < Rsqr;
				//bool b1 = ((c1-E.m_b)^(c1-E.m_b)) < Rsqr;
				
				//if ((a1||a2) && (b1||b2)) return false;
				//if ((a1||a2) && !(b1||b2)) continue;
				//if (!(a1||a2) && (b1||b2)) continue;
				
				return false;
			}
		}
		return true;
	}
	
	
	/*
	bool triangleFits(CandidateTriangle& candTriangle, Geometry& mesh)
	{
		return true;
		
		
		// Make circumcircle.   Make AABB of circumcircle.
		// Test AABB against mesh.  If no hit, we're done, return true.
		// If there is a hit, get all the vertices of the mesh triangles in the list.
		// Check if any of them are inside the circumcircle. If so, return false, otherwise return true.
		
		/// NO: MUST TEST WHETHER SOME TRIANGLE *INTERSECTS* THE CIRCUMCIRCLE ... HMMM
		
		// If any edge of the existing mesh passes through the circumcircle, *ignoring the edges adjacent to the test triangle*,
		// then the triangle must be rejected.
		

		float R;
		Vec4 Center;
		Polygonizer::computeCircumcircle(candTriangle.m_A, candTriangle.m_B, candTriangle.m_C, R, Center);
		
		float Rsqr = R*R;
		
		V3f CenterV(Center[0], Center[1], Center[2]);
		Box3f testAabb;
		testAabb.extendBy(CenterV);
		testAabb.max += V3f(R);
		testAabb.min -= V3f(R);
	
		vector<int> triIndices;
		m_BVH.query(testAabb, triIndices);
		for (int t=0; t<triIndices.size(); ++t)
		{
			const Geometry::Triangle& meshTriangle = mesh.m_triangles[triIndices[t]];
			for (int a=0; a<3; ++a)
			{
				const Vec4& meshVertex0 = mesh.m_vertices[meshTriangle.m_vertex[a]];
				const Vec4& meshVertex1 = mesh.m_vertices[meshTriangle.m_vertex[(a+1)%3]];
				
				// If either meshVertex0 or meshVertex1 "lies in the active edges which are in the neighborhood of both
				// active edges which contain the point pnew", we can ignore this edge.
				
				// If either meshVertex0 or meshVertex1 is identical to one of our triangle vertices, we ignore this mesh edge
				// as it does not represent a colliding triangle.
				Vec4 dA0 = meshVertex0 - candTriangle.m_A;
				Vec4 dB0 = meshVertex0 - candTriangle.m_B;
				Vec4 dC0 = meshVertex0 - candTriangle.m_C;
				Vec4 dA1 = meshVertex1 - candTriangle.m_A;
				Vec4 dB1 = meshVertex1 - candTriangle.m_B;
				Vec4 dC1 = meshVertex1 - candTriangle.m_C;
				
				if (dA0*dA0<FLT_EPSILON) continue;
				if (dB0*dB0<FLT_EPSILON) continue;
				if (dC0*dC0<FLT_EPSILON) continue;
				if (dA1*dA1<FLT_EPSILON) continue;
				if (dB1*dB1<FLT_EPSILON) continue;
				if (dC1*dC1<FLT_EPSILON) continue;
				
				// Does edge meshVertex0 <-> meshVertex1 intersect the circumcircle?
				if ( intersectRaySphere(meshVertex0, meshVertex1-meshVertex0, R, Center) )
				{
					return false;
				}
				
				Vec4 dV0 = meshVertex0 - Center;
				Vec4 dV1 = meshVertex1 - Center;
				if (dV0*dV0 < Rsqr) return false;
				if (dV1*dV1 < Rsqr) return false;
			}
		}
		return true;

	}
	 
	 */
	
	void drawFront()
	{
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		
		glDisable(GL_TEXTURE_2D);
		glEnable(GL_DEPTH_TEST);
			
		glDisable(GL_CULL_FACE);
		glCullFace(GL_BACK);
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
		
		Vec4 color(1,0,0,1);
		glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, &color[0]);
		glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, &color[0]);
		
		glDisable(GL_LIGHTING);
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	
		glColor4f(1,0,0,0.5);
		glBegin(GL_LINE_LOOP);
		
		list<BoundaryTriangle>::iterator edge = m_fronts[0].m_boundaryTriangles.begin();
		while (edge != m_fronts[0].m_boundaryTriangles.end())
		{
			int a = edge->m_a;
			int b = edge->m_b;
			const Vec4& A = m_mesh.m_vertices[a];
			const Vec4& B = m_mesh.m_vertices[b];
			
			glVertex3fv(&A[0]);
			glVertex3fv(&B[0]);
			
			edge++;
		}
		
		glEnd();
		
		glPopAttrib();
	}
	
	
	
	template<typename Func>
	bool grow(Front& front, Geometry& mesh, float distanceEpsilon, float tol)
	{		
		list<BoundaryTriangle>::iterator boundaryTriangle = m_currentBoundaryTriangle; //front.m_edges.begin();
		if (boundaryTriangle == front.m_boundaryTriangles.end())
		{
			boundaryTriangle++;
		}
		
		bool triangleAdded = false;
		
		//while (edge != front.m_edges.end()) // && mesh.m_vertices.size()<128)
		{
			int a = boundaryTriangle->m_a;
			int b = boundaryTriangle->m_b;
			int c = boundaryTriangle->m_otherTriangleVertex;
			
			const Vec4& A = mesh.m_vertices[boundaryTriangle->m_a];
			const Vec4& B = mesh.m_vertices[boundaryTriangle->m_b];
			const Vec4& C = mesh.m_vertices[boundaryTriangle->m_otherTriangleVertex];

			// Construct a new candidate point
			Vec4 midPoint = 0.5f * (A + B);
			Vec4 centroid = 0.3333333f * (A + B + C);
			
			Vec4 n = (centroid - A) % (B - A);
			Vec4 outVec = n % (B - A);
			outVec.normalizeIfNotZero();
			
			// This is where we should be intelligently choosing a good length based on 
			// the local curvature ...   for now, just do:
			float extrapolationLength = sqrt(3.f)/2.f * (B - A).length3();		
			
			Vec4 candidatePoint = midPoint + extrapolationLength*outVec;

			// Project it to the isosurface
			const int maxIterations = 100;
			Vec4 projectedPoint = project<Func>(candidatePoint, tol, distanceEpsilon, maxIterations);
			
			// Test this triangle for acceptability (according to "Delaunay criterion"):
			// a) construct the sphere centered at the triangle centroid which just contains the three vertices
			// b) find whether the sphere touches any other vertices in the current mesh.
			//float circumradius;
			//Vec4 circumcenter;
			//computeCircumcircle(A, B, projectedPoint, circumradius, circumcenter);
			// TO DO: debug, draw circumcircle
			
			/// Make bounding box of circumcircle. Use AABB trees to check if box hits some triangles.
			// Check the circumcircle exactly against those triangles
			// FOR NOW: let's just do an AABB test

			Edge E0(A, projectedPoint);
			Edge E1(B, projectedPoint);
			
			if ( edgeFits(E0) && edgeFits(E1) )
			{
				 // New triangle (A, B, projectedPoint)
				 int newVertexIndex = mesh.m_vertices.size();
				 int newTriangleIndex = mesh.m_triangles.size();
				 
				 Geometry::Triangle tri;   // TO DO: think about winding...
				 tri.m_vertex[0] = boundaryTriangle->m_a;
				 tri.m_vertex[1] = boundaryTriangle->m_b;
				 tri.m_vertex[2] = newVertexIndex;
				 mesh.m_triangles.push_back(tri);
				 mesh.m_vertices.push_back(projectedPoint);
				 
				 BoundaryTriangle newTri0(boundaryTriangle->m_a, newVertexIndex, boundaryTriangle->m_b, newTriangleIndex);
				 BoundaryTriangle newTri1(newVertexIndex, boundaryTriangle->m_b, boundaryTriangle->m_a, newTriangleIndex);
				
				 // Modify --> edge -->  to  --> edge0 --> edge1 -->	
				 list<BoundaryTriangle>::iterator nextEdge = boundaryTriangle; nextEdge++;
				 if (nextEdge == front.m_boundaryTriangles.end()) nextEdge++;
				 list<BoundaryTriangle>::iterator nextEdgeToProcess = front.m_boundaryTriangles.insert(boundaryTriangle, newTri0);
				 front.m_boundaryTriangles.insert(nextEdge, newTri1);
				 front.m_boundaryTriangles.erase(boundaryTriangle);
				 				
				 m_BVH.insertEdge(E0);
				 m_BVH.insertEdge(E1);

				 boundaryTriangle = nextEdgeToProcess;
				 boundaryTriangle++; // ?? DEBUG
				 boundaryTriangle++; // ?? DEBUG
				
				 triangleAdded = true;
			}
			else
			{
				// Try candidate triangles created from the existing adjacent vertices 
				list<BoundaryTriangle>::iterator prevEdge = boundaryTriangle; prevEdge--;
				if (prevEdge == front.m_boundaryTriangles.end()) prevEdge--;
				assert(prevEdge->m_b == boundaryTriangle->m_a);
				const Vec4& prevVertex = mesh.m_vertices[prevEdge->m_a];
				
				list<BoundaryTriangle>::iterator nextEdge = boundaryTriangle; nextEdge++;
				if (nextEdge == front.m_boundaryTriangles.end()) nextEdge++;
				assert(nextEdge->m_a == boundaryTriangle->m_b);
				const Vec4& nextVertex = mesh.m_vertices[nextEdge->m_b];

				Edge Eprev(prevVertex, B);
				Edge Enext(A, nextVertex);
								
				if ( edgeFits(Eprev) )
				{
					Geometry::Triangle tri;
					tri.m_vertex[0] = prevEdge->m_a;   // TO DO: think about winding...
					tri.m_vertex[1] = boundaryTriangle->m_a;
					tri.m_vertex[2] = boundaryTriangle->m_b;
					int newTriangleIndex = mesh.m_triangles.size();
					mesh.m_triangles.push_back(tri);
					
					// Insert the new edge and remove the two edges it replaces
					BoundaryTriangle newEdge(prevEdge->m_a, boundaryTriangle->m_b, boundaryTriangle->m_a, newTriangleIndex);
					list<BoundaryTriangle>::iterator nextEdgeToProcess = front.m_boundaryTriangles.insert(nextEdge, newEdge);
					front.m_boundaryTriangles.erase(boundaryTriangle);
					front.m_boundaryTriangles.erase(prevEdge);
					
					m_BVH.insertEdge(Eprev);
					
					boundaryTriangle = nextEdgeToProcess;
					boundaryTriangle++; // ?? DEBUG
					
					triangleAdded = true;
				}
				else if ( edgeFits(Enext) )
				{
					Geometry::Triangle tri;
					tri.m_vertex[0] = nextEdge->m_b;    // TO DO: think about winding...
					tri.m_vertex[1] = boundaryTriangle->m_a;
					tri.m_vertex[2] = boundaryTriangle->m_b;
					int newTriangleIndex = mesh.m_triangles.size();
					mesh.m_triangles.push_back(tri);
					
					// Insert the new edge and remove the two edges it replaces
					BoundaryTriangle newEdge(boundaryTriangle->m_a, nextEdge->m_b, boundaryTriangle->m_b, newTriangleIndex);
					list<BoundaryTriangle>::iterator nextnextEdge = nextEdge; nextnextEdge++;
					list<BoundaryTriangle>::iterator nextEdgeToProcess = front.m_boundaryTriangles.insert(nextnextEdge, newEdge);
					front.m_boundaryTriangles.erase(boundaryTriangle);
					front.m_boundaryTriangles.erase(nextEdge);
					
					m_BVH.insertEdge(Enext);
					
					boundaryTriangle = nextEdgeToProcess;
					boundaryTriangle++; // ?? DEBUG
					
					triangleAdded = true;
				}
				else
				{
					boundaryTriangle++;
				}
			}			
		}
		
		m_currentBoundaryTriangle = boundaryTriangle;
		
		// We now have gone as far as we can from this seed, without pasting the cracks.
	
		return triangleAdded;
	}
	
	
	Polygonizer(long seed=-1) : m_seed(seed), m_BVH(8)
	{
		ran2(&m_seed);	
	}
	
	
	Geometry m_mesh;
	
	float m_distanceEpsilon;
	float m_tol;
	
	// DEBUG
	list<BoundaryTriangle>::iterator m_currentBoundaryTriangle;
};





inline void computeTangentSpace(const Vec4& N, Vec4& T, Vec4& B)
{
	// (N assumed normalized)
	float nx = N[0];
	float ny = N[1];
	float nz = N[2];
	if (nx>0.5f || ny>0.5f) 
		T = Vec4(ny, -nx, 0.f);
	else
		T = Vec4(-nz, 0.f, nx);
	B = N % T;
	T.normalizeIfNotZero();
	B.normalizeIfNotZero();
}




template<typename Func>
void Polygonizer::polygonize(const Aabb& bbox, const float tol)
{
	// Look for a good start point
	Vec4 extents = bbox.m_maxima - bbox.m_minima;
	const int startPointSearchIters = 32*32*32;
	float minValue = FLT_MAX;
	Vec4 startPoint;
	for (int n=0; n<startPointSearchIters; ++n)
	{
		Vec4 testPoint = bbox.m_minima + ran2(&m_seed) * extents;
		float testVal = fabs(Func::value(testPoint));
		if ( testVal<minValue ) 
		{
			startPoint = testPoint;
			minValue = testVal;
		}
	}
	
	m_tol = tol;
	m_distanceEpsilon = tol * extents.length3();
	const int maxIterations = 100;
	
	// Project it to the isosurface
	Vec4 seedPoint = project<Func>(startPoint, tol, m_distanceEpsilon, maxIterations);
	
	float inv2D = 0.5f/m_distanceEpsilon;
	Vec4 grad = gradient<Func>(seedPoint, m_distanceEpsilon, inv2D);
	
	Vec4 n = grad;
	n.normalizeIfNotZero();
	
	Vec4 t, b;
	computeTangentSpace(n, t, b);
	
	// Compute first fundamental form
	float E = t * grad; 
	float G = b * grad; 
	float F = E * G;
	E *= E; G *= G;
	
	// Compute second fundamental form
	Matrix3 H = hessian<Func>(seedPoint, m_distanceEpsilon, inv2D);
	
	float L = t * (H * t);
	float N = b * (H * b);
	float M = t * (H * b);
	
	// Compute principal curvatures
	float m00 = L*G - M*F;
	float m01 = M*G - N*F;
	float m10 = M*E - L*F;
	float m11 = N*E - M*F;
	
	float trace = m00+m11;
	float radical = trace*trace - 4.f*(m00*m11-m01*m10);
	if (radical<0.0f)
	{
		// TO DO: deal with this
		printf("Unable to diagonalize shape operator."); 
		exit(1);
	}
	
	radical = sqrt(radical);
	float k1 = 0.5f * (trace + radical);
	float k2 = 0.5f * (trace - radical);
	float meanCurvature = 0.5f * (k1 + k2);
	
	// thus compute candidate triangles for expansion
	
	/////////////////////// DEBUG ///////////////////////
	float radiusOfCurvature = 0.05f; //1.f/meanCurvature;
	/////////////////////// DEBUG ///////////////////////
	
	// stamp initial hexagon
	m_mesh.m_vertices.push_back(seedPoint);
	
	// Generate initial edge list
	for (int i=0; i<6; ++i)
	{
		float x = radiusOfCurvature * cos(float(i) * 0.3333333f*M_PI);
		float y = radiusOfCurvature * sin(float(i) * 0.3333333f*M_PI);
		m_mesh.m_vertices.push_back(seedPoint + x*t + y*b);
	}
	
	Front front;
	for (int i=0; i<6; ++i)
	{
		BoundaryTriangle triangle(1+(i%6), 1+((i+1)%6), 0, i);
		front.m_boundaryTriangles.push_back(triangle);
	}
	
	m_fronts.push_back(front);

	for (int i=0; i<6; ++i)
	{
		Geometry::Triangle tri;                                                                                                                       
		tri.m_vertex[0] = 0;
		tri.m_vertex[1] = 1+(i%6);
		tri.m_vertex[2] = 1+((i+1)%6);
		m_mesh.m_triangles.push_back(tri);
		
		const Vec4& A = m_mesh.m_vertices[tri.m_vertex[0]];
		const Vec4& B = m_mesh.m_vertices[tri.m_vertex[1]];
		const Vec4& C = m_mesh.m_vertices[tri.m_vertex[2]];
		m_BVH.insertEdge(Edge(B, C));
	}
	
	// DEBUG
	
	m_currentBoundaryTriangle = m_fronts[0].m_boundaryTriangles.begin();

	// loop:
	
	
	// Iterate over patches P:
	
	//		Iterate over each front F of patch P:
	
	//				Iterate over vertices V of F
	
	//					Attempt to 'expand' front at vertex V (make edge including V, make triangle from other point 
	//					at candidate place based on curvature + tangent plane + projection)
	
	//						If we find that this would lead to an intersection (or close collision) with some patch Q 
	//						(based on some conservative critertion):
	
	//								If the intersected patch Q != P,  merge fronts of Q, merge patches
	//								If the intersected patch Q == P,  split fronts of Q [do not split patch]
	
	//								[+ potentially logic to prevent problems if multiple collisions happen ...]
	//								[+ special case to eliminate the 'final' three vertex fronts ...]
	                        
	
	// At the end of this, all fronts should have been merged, and shrunk to 3 vertex fronts, then eliminated completely,
	// leaving a single patch with no boundaries.  This is your polygonized isosurface...
	                                                                                                                                                                             
	
}
          
#endif


#endif //POLYGONIZER_H






